Road vertical contour detection

ABSTRACT

Various driver assistance systems mountable in a host vehicle and computerized methods for detecting a vertical deviation of a road surface. The driver assistance system includes a camera operatively connectible to a processor. Multiple consecutive image frames are captured from the camera including a first image of the road and a second image of the road. Based on the host vehicle motion, the second image is warped toward the first image to produce thereby a warped second image. Image points of the road in the first image and corresponding image points of the road in the warped second image are tracked. Optical flow is computed between the warped second image to the first image. The optical flow is compared with an optical flow based on a road surface model to produce a residual optical flow. The vertical deviation is computed from the residual optical flow.

CROSS REFERENCE TO RELATED APPLICATIONS

The present application claims priority from U.S. provisionalapplication 61/567,132 filed Dec. 6, 2011 and from U.S. provisionalapplication 61/727,722 filed Nov. 18, 2012; U.S. provisional application61/727,755 filed Nov. 19, 2012 and is a continuation application ofnon-provisional application Ser. No. 13/693,713 filed Dec. 4, 2012, thedisclosures of which are included herein by reference.

BACKGROUND

1. Technical Field

The present invention relates to driver assistance systems and methodsto detect the vertical deviation of a contour of a road using a camera.

2. Description of Related Art

During the last few years camera based driver assistance systems (DAS)have been entering the market; including lane departure warning (LDW),automatic high-beam control (AHC), traffic sign recognition (TSR)forward collision warning (FCW) and pedestrian detection.

BRIEF SUMMARY

Various driver assistance systems mountable in a host vehicle andcomputerized methods are provided for herein for detecting a verticaldeviation of a road surface. The methods are performable by a driverassistance system mountable in a host vehicle while the host vehicle ismoving. The driver assistance system includes a camera operativelyconnectible to a processor. Multiple consecutive image frames arecaptured from the camera including a first image of the road and asecond image of the road. Based on the host vehicle motion, the secondimage is warped toward the first image to produce thereby a warpedsecond image. Image points of the road in the first image andcorresponding image points of the road in the warped second image aretracked. Optical flow is computed between the warped second image to thefirst image. The optical flow is compared with an optical flow based ona planar or bi-quadratic road surface model of the road to producethereby a residual optical flow. The vertical deviation of the roadsurface is computed from the residual optical flow.

The driver assistance system is operable while the host vehicle ismoving to detect a vertical deviation in contour of a road. A firstimage frame and a second image frame are captured in the field of viewof the camera. Image motion is processed between respective images ofthe road derived from the first image frame and the second image frame.The vertical contour of the road is estimated using a road surface modelof the road and the deviation in the vertical contour is computed fromthe road surface model. The optical flow may be estimated betweenmultiple first image patches of the road derived from the first imageframe and corresponding second image patches of the road derived fromthe second second image frame. The vertical deviation in the roadcontour is determined by comparing the optical flow with an optical flowas predicted by the road surface model. The residual optical flowindicates the deviation in vertical contour of the road.

A third image frame may be captured in the field of view of the cameraand image motion between respective images of the road may be derivedfrom the third image frame and one or more of the first and second imageframes. A multi-frame road surface model may be computed by combining aroad profile of the road derived from said road surface model based onsaid first image frame and said second image frame with said secondprocessing.

The multi-frame road surface model may be mapped from the first and/orsecond image frames to the third image frame by using a homographybetween said at least one previous image frame to the third image frame.

Assuming a planar model for the contour of the road, the image motion ofthe images of the road may be processed by initially warping the secondimage frame toward the first image frame to produce a warped secondimage frame. The initial warp may include aligning the second imageframe with the first image frame by adjusting for an image shift due tomotion of the vehicle relative to the road, yaw, pitch and/or roll. Theinitial warp may include an adjustment for the relative scale changebetween the second image frame and the first image frame. The relativescale change arises from different distances to the camera.

Multiple image points may be selected in the first image frame. Theimage points may be located on the image of the road surface and may belocated at points of a fixed grid. For the image points, multiple imagepatches are located disposed respectively about the image points. Theimage points may be tracked by correlating the image patches in thefirst image frame with corresponding image patches in the warped secondimage frame to produce multiple tracked points. The tracked points arefit to a homography. A refined warp of the warped second image frametoward the first image frame may be performed to correct the initialwarp by using the homography and to produce a refinely warped secondimage frame. Optical flow may be computed between the refinely warpedsecond image frame and the first image frame. The optical flow iscompared with a road surface optical flow based on a road surface model.The deviation in vertical contour of the road produces a residualoptical flow different from the road surface optical flow as found bythe road surface model.

The foregoing and/or other aspects will become apparent from thefollowing detailed description when considered in conjunction with theaccompanying drawing figures.

BRIEF DESCRIPTION OF THE DRAWINGS

The invention is herein described, by way of example only, withreference to the accompanying drawings, wherein:

FIGS. 1 and 2 illustrate a system including a camera or image sensormounted in a vehicle, according to an aspect of the present invention.

FIG. 3 shows a flow diagram of a simplified method for real timemeasurement of vertical contour of a road while a vehicle is movingalong a road, according to a feature of the present invention.

FIG. 4 shows two consecutive image frames captured from a forwardlooking camera mounted in a vehicle, according to a feature of thepresent invention.

FIG. 5 includes a flow chart showing details of a processing step shownin FIG. 3, according to a feature of the present invention.

FIG. 6 includes a flow chart illustrating further details of an initialwarping step shown in FIG. 5, according to feature of the presentinvention.

FIG. 7a shows the results of the initial warp step of FIG. 5, theresults shown as a warped image, according to a feature of the presentinvention.

FIG. 7b shows a difference image as a result of the difference betweenthe warped image of FIG. 7a and an image, according to a feature of thepresent invention.

FIG. 8a shows a warped image frame with a trapezoidal region, accordingto a feature of the present invention.

FIG. 8b shows a detail of the trapezoidal region in warped image frameof FIG. 8a , according to a feature of the present invention.

FIG. 9a shows the results of the refined warp of a warped image towardsan image, according to a feature of the present invention.

FIG. 9b shows the difference between the refined warp of a warped imagetowards an image and the image, according to a feature of the presentinvention.

FIG. 10a shows the results of tracking a dense grid of points, accordingto a feature of the present invention.

FIGS. 10b, 10c and 10d show details of areas indicated in FIG. 10a ,according to a feature of the present invention.

FIG. 11 shows two filtered and cut images that are fed into an opticalflow routine executed in Matlab™, according to a feature of the presentinvention.

FIG. 12 shows the y component of the residual optical flow as a grayscale image, according to a feature of the present invention.

FIG. 13 shows the same data as shown in FIG. 12 overlaid on an originalimage, according to a feature of the present invention.

FIG. 14 which shows a graph of image y co-ordinate versus planar motionflow in the y direction, according to a feature of the presentinvention.

FIG. 15 shows a graph of road profile in meters versus distance from acamera in meters, according to a feature of the present invention.

DETAILED DESCRIPTION

Reference will now be made in detail to features of the presentinvention, examples of which are illustrated in the accompanyingdrawings, wherein like reference numerals refer to the like elementsthroughout. The features are described below to explain the presentinvention by referring to the figures.

Before explaining features of the invention in detail, it is to beunderstood that the invention is not limited in its application to thedetails of design and the arrangement of the components set forth in thefollowing description or illustrated in the drawings. The invention iscapable of other features or of being practiced or carried out invarious ways. Also, it is to be understood that the phraseology andterminology employed herein is for the purpose of description and shouldnot be regarded as limiting.

Reference is now made to FIGS. 1 and 2 which illustrate a system 16including a camera or image sensor 12 mounted in a vehicle 18, accordingto an aspect of the present invention. Image sensor 12, imaging a fieldof view in the forward direction provides image frames 15 in real timeand image frames 15 are captured by an image processor 30. Processor 30may be used to process image frames 15 simultaneously and/or in parallelto serve a number of driver assistance systems/applications. Processor30 may be used to process image frames 15 to detect and recognize animage or portions of the image in the forward field of view of camera12. The driver assistance systems may be implemented using specifichardware circuitry (not shown) with on board software and/or softwarecontrol algorithms in storage 13. Image sensor 12 may be monochrome orblack-white, i.e. without color separation or image sensor 12 may becolor sensitive. By way of example in FIG. 2, image frames 15 are usedto serve pedestrian detection 20, traffic sign recognition (TSR) 21,forward collision warning (FCW) 22 and real time detection 23 of thevertical contour of the road or deviation from the road plane accordingto features of the present invention.

In some cases, image frames 15 are partitioned between different driverassistance applications and in other cases the image frames 15 may beshared between the different driver assistance applications.

By way of introduction, various embodiments of the present invention areuseful to accurately detect road shape i.e. vertical profile of a roadusing camera 12 mounted in host vehicle 18. Using systems and methodsprovided herein, bumps and/or holes such as speed bumps, curbs andmanhole covers may be detected with vertical deviations as little as twocentimeters from the road plane. System and methods as disclosed hereinmay be similarly applied to forward viewing, side viewing and rearviewing cameras 12.

Various methods as described herein accurately estimate the planar (orbi-quadratic) model of the road surface and then computes the smalldeviations from the planar (or bi-quadratic) model to detect bumps andholes.

Reference is now made to FIG. 3 which shows a flow diagram of simplifiedmethod 23 for real time measurement of vertical contour of a road whilevehicle 18 is moving along a road, according to a feature of the presentinvention. In step 303, a first image frame 15 and a second image frame15 are captured of a road in the field of view of camera 12 mounted invehicle 18. Image motion from first image frame 15 to second image frame15 is processed (step 305) to detect a vertical deviation of contour ofthe road. Further details of step 305 are shown in the description thatfollows.

Reference is now also made to FIG. 4 which shows two consecutive imageframes 15 a and 15 b captured (step 303) from forward looking camera 12mounted in a vehicle 18, according to a feature of the presentinvention. Image frame 15 b is captured after image frame 15 a iscaptured. Equivalently image frame 15 b may be captured prior tocapturing image frame 15 a Camera 12 in the description that follows maybe a WVGA camera (Aptina M9V024 and Sunny 4028A 5.7 mm lens) as used inthe Mobileye™ advance warning system (AWS)™.

Reference is now also made to FIG. 5 which includes a flow chart showingdetails of processing step 305, according to a feature of the presentinvention. The term “warping” as used herein refers to a transform fromimage space to image space.

Image frame 15 b is initially warped (step 501) into image frame 15 a.(In a similar process, image frame 15 a may be initially warped intoimage frame 15 b). It is assumed that a road can be modeled as an almostplanar surface. Thus imaged points of the road will move in image spaceaccording to a homography. The term “homography” as used herein refersto an invertible transformation from a projective space to itself thatmaps straight lines to straight lines. In the field of computer vision,two images of the same planar surface in space are related by ahomography assuming a pinhole camera model.

In particular, by way of example, for a given camera 12 height (1.25 m),focal length (950 pixels) and vehicle motion between frames (1.58 m), itmay be possible to predict the motion of the points on the road planebetween the two image frames 15 a and 15 b respectively. Using a modelof the almost planar surface for the motion of the road points, it ispossible to warp the second image 15 b towards the first image 15 a. Thefollowing Matlab™ code would perform initial warp step 501:

[h,w]=size(Iin); Iout=zeros(size(Iin)); for i=1:h, for j=1:w, x=j; y=i;S=dZ/(f*H); x1=x(:)−x0; y1=y(:)−y0; y2=y1./(1+y1*S); x2=x1./(1+y1*S);x2=x2+x0; y2=y2+y0; Iout(i,j)=bilinearInterpolate(Iin,x2,y2); end; end;where dZ is the forward motion of vehicle 18, H is camera 12 height andf is the focal length of camera 12. p₀=(x₀; y₀) is the vanishing pointof the road structure. Alternatively, it may be may be possible to useinitial calibration values during installation of of system 1 in vehicle18, where x₀ is the forward direction of the vehicle 18 and y₀ is thehorizon line when vehicle 18 is on a horizontal surface. The variable Sis an overall scale factor relating image coordinates between the twoimage frames 15 a and 15 b captured at different vehicle distances Zfrom camera 12. The term “relative scale change” as used herein refersto the overall scale change in image coordinates dependent upon distanceZ to camera 12.

Reference is now made to FIG. 6 which includes flow chart illustratingfurther details of initial warping step 501, according to feature of thepresent invention. According to the road model, image 15 b istransformed by rotation (step 601) towards image 15 a according to anestimate of yaw, pitch and roll that are available. The estimate maycome from inertial sensors such as a yaw rate sensor in vehicle 18 or incamera 12 head. The estimate might also come from values computed fromprevious image frames 15. Initial warping based on planar road planemodel in shown in step 603.

In practice rotation (step 601) and the road model warp (step 603) canbe combined into a single warp so only one step of bilinearinterpolation is required. If only pitch and yaw are involved these canbe approximated by image shifts. For example, yaw can be approximated ahorizontal image shift δθ_(Pixels) from equations 1 and 2:

$\begin{matrix}{{{\delta\Theta} = {\delta\; t \times {yawRate}}};} & (1) \\{{\delta\Theta}_{Pixels} = \frac{f\;{\delta\Theta}*\pi}{180}} & (2)\end{matrix}$

Reference is now made to FIG. 7a which shows the results of the initialwarp step 501 of image 15 b towards image 15 a, the results are shown aswarped image 15 w, according to a feature of the present invention. Thewarp may be based on vehicle 18 motion (from the speedometer, inertialsensors etc.).

Reference is now made to FIG. 7b which shows the difference betweenwarped image 15 w and image 15 a shown as image 15 d, according to afeature of the present invention. In FIG. 7b it can be seen that somefeatures on the road are still not perfectly aligned.

Tracking of Points

After initial warp (step 501), the remaining motion of features on theroad can be approximated locally, as a uniform translation of an imagepatch from image 15 a to image 15 w. This is not true of the motionbetween the original image 15 a and un-warped image 15 b, where themotion of a patch also involves a non-uniform scale change.

Reference is now also made to FIG. 8a which shows warped image frame 15w with a trapezoidal region 80, according to a feature of the presentinvention. Instead of trying to find feature points, which wouldinvariably give a bias towards strong features such as lane marks andshadows, a fixed grid 84 of points is used for tracking (step 507). Grid84 of points are selected (step 503) from a trapezoidal region 80 thatroughly maps to up 15 meters ahead and one lane in width. Points 84 arespaced every 20 pixels in the horizontal direction and 10 pixels in thevertical direction. An alternative would be to randomly select pointsaccording to a particular distribution.

Around each point in image 15 a a patch is located (step 505). In thiscase the patch is 8 pixels in each direction centered around the pointresulting in a 17×17 pixel square. The normalized correlation is thencomputed (e.g. Matlab™ function normxcorr2) for warped image 15 w, wherethe patch center is shifted in the search region. In practical use theremay be a yaw sensor but no pitch sensors and so a tighter search regionis used in the x direction rather than in the y direction. A searchregion of (2×4+1) pixels in the x direction may be used and (2×10+1)pixels in the y direction.

The shift which gives the maximum correlation score was found and wasfollowed by a refinement search around the best score position with asub-pixel resolution of 0.1 pixels. This refinement step gave superiorresults to trying to fit the integer scores around the maximum score toa parabolic surface or spline and using these integer scores around themaximum score to compute a sub-pixel match. The refinement search with asub-pixel resolution of 0.1 pixels also gave better results than LukasKanade flow which minimizes the sum square differences. Invalid tracksmay be filtered out at the search stage by picking those points with ascore above a threshold (e.g. T=0.7) leaving tracked points 509 as aresult of tracking step 507 and that the reverse tracking from warpedimage 15 w to image 15 a gives a similar value in the oppositedirection. Reverse tracking is similar to left-right validation instereo.

Reference is now also made to FIG. 8b which shows a detail 82 oftrapezoidal region 80 in warped image frame 15 w, according to a featureof the present invention. Triangle and circle points 84 are the startinglocation. Diamond points are the corresponding tracked location. Inliersare shown as circles and outliers are shown as triangles.

Robust Fitting

Tracked points 509 as a result of tracking step 507, are fit to ahomography (step 511) using RANdom SAmple Consensus (RANSAC). A number,e.g. four, of points are chosen at random and used to compute thehomography. Points 509 are then transformed using the homography and thenumber of points which are closer than a threshold are counted. Randomlychoosing 4 points and counting the number of points which are closerthan a threshold is repeated many times and the four points that gavethe highest count are retained.

At the end of the process, the four best points are used to again (step513) transform the points and all the points (inliers) that are closerthan a (possibly different) threshold are used to compute a homographyusing least squares. The rest of the points that are not closer than a(possibly different) threshold are considered outliers.

At this point in the process, the number of inliers and their spread inwarped image 15 w give an indication to the success of finding the roadplane model. It is usual to get over 100 inliers and a good fit. FIG. 8bshows the inliers as circles and outliers as triangles. The homographycan then be used to correct the initial alignment warp (step 501).Correction of the initial alignment warp can be done by integrating thecorrection into the initial warp (step 501) or to do the two warpsconsecutively. The former is advantageous as it requires only oneinterpolation step and can be performed optionally by matrixmultiplication of the two homography matrices.

Reference is now made to FIG. 9a which shows the results of the refinedwarp of warped image 15 w towards image 15 a, according to a feature ofthe present invention. In FIG. 9a , features on the road are almostperfectly aligned. There are however, still some brightness differencesthat are not accounted for.

Reference is now made to FIG. 9b which shows the difference between therefined warp of warped image 15 w towards image 15 a and image 15 a,according to a feature of the present invention.

Dense Tracking and Residual Flow

After warping image 15 b towards image 15 a to give warped image 15 w,using the refined warp (step 513), the tracking of points (step 507) maybe repeated using a finer grid (e.g. every 5th pixel on every 5th row)and over a wider region of the road. Since the road plane is very wellaligned, a smaller region may be searched over such as 2 pixels in eachdirection, again, with a subpixel search.

Reference is now made to FIG. 10a which shows the results of tracking adense grid of points, according to a feature of the present invention.FIG. 10a includes area 124 with a car and shadow 104 on the car. Alsoareas 120 and 122 which include manhole cover 100 and sidewalk 102respectively. FIGS. 10b, 10c and 10d show greater details of areas 124,120 and 122 respectively.

Results are shown in FIGS. 10a-10d as a Matlab™ quiver plot. The Matlab™quiver plot displays velocity vectors as arrows with components (u,v) atthe points (x,y). For example, the first vector is defined by componentsu(1),v(1) and is displayed at the point x(1),y(1). Quiver(x,y, u,v)plots vectors as arrows at the coordinates specified in eachcorresponding pair of elements in x and y. Points on the road shouldexhibit flow close to zero. Points on features above the road plane willhave flow greater than zero and points below the road plane will haveresidual flow below zero. Positive flow is defined as flow away from thefocus of expansion (FOE) (generally speaking down and outwards). Notethat the elevated sidewalk 102 as shown in FIG. 10d and sunken manholecover 100 as shown in FIG. 10c both show up well. Objects which are notfixed to the road also show significant residual flow as on the car andthe shadow 104 on the car. Points with flow above 0.5 pixels are shownin the elevated sidewalk 102 and points with flow below −0.5 pixels areshown on the sunken manhole cover 100, on the car and the shadow 104 onthe car.

Sidewalks can thus be detected by detecting relatively homogenouspositive residual flow regions that are separated from areas of lowresidual flow by elongated lines. These lines are roughly heading in thedirection of the FOE or the direction of the road (i.e. the direction ofthe vanishing points of the lane marks).

Significant robustness to noise can be achieved by combining informationfrom neighboring points (i.e. applying some sort of smoothnessconstraint) by using global cost functions with variational approachesor by using dynamic programming

Variational Approaches

As a first, the Horn And Schunk optical flow computation may be appliedbetween image 15a and warped image 15 b. (Horn, B. K. P. & B. G.Schunck, “Determining Optical Flow”, Artificial Intelligence, Vol. 17,No. 1-3, August 1981, pp. 185-203). Since the images are well alignedthe algorithm can converge quite well. Horn and Schunk use quadraticerror functions for both the data and the error terms. Better resultscan be obtained using L1 errors terms. The optical flow code of Ce Liu(C. Liu. Beyond pixels: exploring new representations and applicationsfor motion analysis. Doctoral Thesis. Massachusetts Institute ofTechnology. May 2009. Appendix A pages 143-148) works quite well. Thecode is by Ce Liu based on the work of Brox et al. (T. Brox, A. Bruhn,N. Papenberg, and J. Weickert. High accuracy optical flow estimationbased on a theory for warping. In European Conference on Computer Vision(ECCV), pages 25-36, 2004.)

However, some changes are made to the code of Ce Liu. When using thevariational approach the computation is restricted to only one level ofthe pyramid (the nest or original level) for two reasons:

1. Often the texture on the road is very fine and only the texture canbe seen in the highest resolution image and not in upper levels of thepyramid. Typically there is no coarse texture on the road socoarse-to-fine does not work.

2. The solution should not be pulled towards the significant, coarsescale brightness features. Otherwise the solution will be pulled awayfrom the well aligned starting point and never recover.

A further term may be added to the cost function which penalizes forstraying far from the planar model (i.e. for flow above 1 pixel or someother threshold). The function:λΣ(u²+v²)^(a)   (3)works well where u and v are the values of the flow in x and yrespectively, a determines the shape of the cost term. a=4 works well. λweights this cost term compared to the data term and the smoothnessterm.

In order to reduce the effects of brightness changes between images, abandpass or high-pass filtering may be performed. For example:

-   f=ones(5);-   f=f/sum(f(:));-   f=conv2(f,f);-   f=conv2(f,f);-   f=conv2(f,f);-   im1f=conv2(im1,f,‘same’);-   im2f=conv2(im2,f,‘same’);-   im1=im1-im1f;-   im2=im2-im2f;

The optical flow computation may be applied to only the part of theimage typically occupied by the road. For example by starting only fromthe horizon down, remove 100 columns on the left and right and also thebottom part of image 15 a which does not appear in warped image 15 w(and is zero in the warped image, see FIG. 9a ).

In Matlab™ code that is:

-   -   im1=im1(200:400,100:540);    -   im2=im2(200:400,100:540);

Reference is now made to FIG. 11 which shows two filtered and cut imagesthat are fed into the optical flow routine in the Matlab™ codes above,according to a feature of the present invention. The two images filteredand cropped to be used as input to the optical flow routine have theroad texture enhanced by the high-pass filtering.

Reference is now made to FIG. 12 which shows the y component of theresidual optical flow as a gray scale image, according to a feature ofthe present invention. Note the dark patch centered around (300,140) onthe horizontal and vertical axis respectively. The dark patch is due tothe negative residual flow on manhole cover 100 which is sunken in theroad. The solid lines indicate tracks 0.5 m wide in front of the hostvehicle wheels. The dotted line is the row average of the data betweeneach pair of solid lines. The data is scaled by 30 to make the shape ofthe data visible. Note the significant dip in the right dotted red linedue to the manhole cover 100.

Reference is now made to FIG. 13 which shows the same data as shown inFIG. 12 overlaid on the original image 15 a, according to a feature ofthe present invention. The solid lines indicate tracks 0.5 m wide infront of the host vehicle wheels. The dotted line is the row average ofthe data between each pair of solid lines.

Reference is now made to FIG. 14 which shows a graph of image yco-ordinate versus planar motion flow in the y direction, according to afeature of the present invention. The same information shown in FIGS. 12and 13 can be converted to metric values. First it is assumed that therotation warp, initial warp and refinement warp have been combined intoa single warp. With the single warp, there is now effectively a functionthat accurately maps points on the road from image 15 a to image 15 b.FIG. 14 shows the expected flow in the y direction according to theplanar model. Flow has been plotted on the x axis so that the y axisaligns with the y axis of the warped image 15 w.

The y coordinate of the minimum flow corresponds to y₀ of the imagespace of image 15 a, in other words the horizon or vanishing point ofthe road in image 15 a. In the case of FIG. 14, image co-ordinate y=200.The x coordinate of the minimum is the change in the value for y₀between image 15 a and warped image 15 w. The change in the value for y₀between image 15 a and warped image 15 w is in effect the actual pitchvalue in pixels. In the case shown in FIG. 14 the actual pitch value is10 pixels.

Given y₀ it is easy to translate row coordinates into distance on theroad plane:

$\begin{matrix}{Z = \frac{fH}{y - y_{0}}} & (4)\end{matrix}$

The flow of the road plane due to forward motion alone (i.e. aftercompensating for pitch) is given by the combined warp value minus thepitch. Let v be the flow for the road plane and δv be the residual flow.Then the height of the point from the road δH is given by:

$\begin{matrix}{{\delta\; H} = {\frac{\delta\; v}{v}H}} & (5)\end{matrix}$

Reference is now made to FIG. 15 which shows a graph of road profile inmeters versus distance from camera 12 in meters. FIG. 15 shows the roadprofile of the right hand track of FIG. 12 in metric units of distanceand height.

Dynamic Programming

In the use of dynamic programming it is assumed that for a narrow track,such as a track 0.5 m wide (as shown in FIG. 13) and extending 20 m infront of one of the wheels of vehicle 18, the road surface can bemodeled as a one dimensional function of the distance on the road Z.

For each point in the strip the normalized correlation may be computedin a small search region around the point to sub pixel accuracy. Thenormalized correlation computed in a small search region is similar tothe fine resolution grid described above. However, instead of pickingthe highest correlation for each point as before, the averagecorrelation score along the row is computed for each possible residualflow and robust averaging to remove outliers may used.

Next is to find a function δv as a function of y or δH as a function ofZ, that maximizes the total correlation score and some smoothness score.It is also possible to add a term penalizing for deviations from theplanar model.

A one dimensional optimization problem of this kind leads itself todynamic programming For the first row, the score for each residual flowis computed in the given range (e.g. 2 pixels). Then for each row n+1,the score associated with the average correlation score along row n+1 iscomputed. Also the score for each residual flow which is the best scoretaking into account the cumulative scores in row N and the smoothnessscore between this residual flow and the residual flows in row n iscomputed.

To be more explicit, a N×M table is set up, where N is the number ofimage rows in the track (for example, the 150 rows between 250 and 400)and M is the search space of residual flow. For example the 21 values:[−1:0.1:1]. The first of the N rows is simply the average correlationscore given each residual flow: S_(NC)(1, i) for each residual flow inthe range [−1:0.1:1].T(1, i)=S _(NC)(1, i)   (6)

For the general row n where n=2 to N, the value for table entry T(n, j)is a combination the average correlation score for row n for residualmotion j(S_(NC)(n, j), and the score that maximizes the combination ofT(n −1, i) and the smoothness score S_(sm)(i, j).T(n,j)=Ψ(S _(NC)(n,j),Max,(Φ(T(n−1,i),S _(sm)(i,j))))   (7)where Ψ and Φ are functions that combine the scores. A simple functioncould be addition. After the table has been filled one performsback-tracing to get the optimal path, which describes the surface.Updates to the Single Frame SystemComputation Direction

In earlier versions the later frame was warped towards the earlier frameand the road profile was computed in the earlier frames coordinatesystem. A later version reverses this and warps the earlier imagetowards the most recent image and the road profile is computed in thismost recent coordinate frame. The computation is the same but it has thefollowing advantages:

-   1. Warping the earlier image towards the most recent image gives the    results in the most relevant coordinate frame for the application.-   2. When driving forward (the typical situation) all the road that    appears in the most recent image has been viewed in the earlier    images. There are no ‘black’ regions as appear for example in FIG. 7    b.-   3. It makes it easier to implement the multi-frame concept.

Picking the Frames

The current frame is picked as frame 2 and then a search back is madethrough the frames to find the closest previous frame where the vehiclemotion was above a certain value (for example 1 m). The vehicle motionabove a certain value is based on the vehicle speed and the time stampsof the frames. This frame is denoted frame 1.

Initial Motion Warp

The parameters for the initial motion warp can be determined frominertial sensors or from the images themselves or a mixture of the two.For example, in a typical modern car, the speed is available also yawrate. Pitch rate might not be available and will be estimated from theimage.

It is more convenient to implement the warps using homography matrices.That way the warps can be combined together into a single warp.

At the the initial motion warp stage, approximations can be used such asperforming the yaw and pitch warps as shifts and ignore roll. The lateralignment stage will correct for any affine and projective motion.

The yaw rotation between the images is based on the yaw angle thetaconverted into pixels. A homography matrix is then constructed to shiftthe image:

-   dTheta=dt*yawRate;-   dThetaPix=f*dTheta*pi/180-   dx=round(dThetaPix);-   Hdx=eye(3);-   Hdx(1,3)=dx;

The pitch between the images is determined from the images by trackinglarge patch centered on the horizon (for a high resolution 1280×960pixel image the patch is 400 pixel wide and 200 pixel high). The patchis tracked over ±80 pixels in the vertical direction and the best matchis used as the pitch value. As an alternative the region can betessellated into sub-patches, each path tracked and a median value used.A homography matrix is then constructed to shift the image:

% find pitch and rotate around X axis(approximate as shift) based onpitch

-   dy=findMotionY(I2,I1,y0)-   Hdy=eye(3);-   Hdy(2,3)=dy;

The vehicle speed, focal length and camera height are used to computethe expected road motion. The expected road motion also is a homography:

-   Hs1=eye(3);-   Hs1(1,3)=−x0;-   Hs1(2,3)=−y0;-   S=dZ/(f*H);-   Hs2=eye(3);-   Hs2(3,2)=S;-   Hs3=eye(3);-   Hs3(1,3)=x0;-   Hs3(2,3)=y0;-   Hw=Hs3*Hs2*Hs1;

The three homographies are then multiplied together to form the completetransformation:

-   Hall=Hw*Hdy*Hdx;-   I1hw=homoWarp1(I1orig,Hall);-   I1w=I1hw;    Tracking of Points and Robust Fitting

The result of the RANSAC is the correction homography H2fixed to theinitial warp Hall. The correction homography H2fixed and the initialwarp Hall can be multiplied together to give the accurate homography ofthe road plane from previous image 1 to current image 2.

H2final=Hall*H2fixed

The accurate homography matrix denoted A′ is composed of the camera 12motion:

-   -   (R,{right arrow over (R)})        and the plane normal:    -   {right arrow over (N)}′        the camera-plane distance d′π and the camera matrices K and K′:

$\begin{matrix}{A^{\prime} = {{K\left( {R^{- 1} + \frac{\overset{->}{T}{\overset{->}{N}}^{\prime\; T}}{d_{\pi}^{\prime}}} \right)}K^{\prime - 1}}} & (8)\end{matrix}$

The (′) is used to denote terms in the coordinate system of the secondimage. Since the camera matrices K and K′ are the same and are known,the homography matrix A′ can be broken down into its components:

$R,{\overset{->}{N}\mspace{14mu}{and}\mspace{14mu}\frac{\overset{->}{T}}{d_{\pi}^{\prime}}}$Dense Tracking and Residual Flow

Instead of computing the dense flow over the whole image, the path ofthe vehicle is predicted based on yaw rate and steering angle and giventhe left and right wheel positions relative to the camera. For eachwheel, a path of width 0.5 m for example, is projected on to the imageusing the parameters of the plane. For every fifth image row along thispath, the path width is divided into 11 grid points which representevery 0.05 m

The path width divided into 11 grid points which represent every 0.05 m,gives a well defined set of grid points along the path (of each wheel).Tracking is performed between image 2 and the warped image 1, forpatches of size 17×17 or 9×9 pixels centered around each grid point. Asearch region of ±8 pixels in both x and y is used, including sub-pixelresolution search in the y direction. Sub-pixel resolution search in thex direction can also be performed but tests did not show improvedperformance and sub-pixel resolution search in the x direction increasescomputation time. As an optimization, the search region for each pixelcan be optimized based on each pixel position in the image and thelocation of the focus of expansion (FOE), since the flow is expectedonly on lines passing through the FOE.

An alternative optimization would be to rectify the images so that theFOE is mapped to infinity, the viewpoint is mapped to an overhead viewpoint and the flow becomes vertical. The alternative optimization isvery similar to rectification in two camera stereo systems. However,given that the transformation on the image is quite different fordifferent distances along the road, it would be advantageous to performthe rectification separately for horizontal strips in the image. Foreach strip there would be one row where the width does bot changesignificantly. Row above the one row would extend and rows below wouldshorten. For example, for a road region of 5 m to 20 m one can warp onestrip 5 m to 10 m centered around 7 m. A second strip can be 8 m to 16 mcentered around 11 m and a third strip can be from 14 m to 22 m centeredaround 17 m.

The strips would not extend the whole width of the image but only wideenough to cover the wheel tracks with a margin to allow for the patchsize.

For each point a validity bit is set based on forward-backwardverification, a minimal correlation value and thresholds on allowed xand y flow values. For each row, the median of the valid points out ofthe 11 points along the row is taken as the value for that row. Thenumber of valid points along the row is a confidence value. For a rowwith no valid points a residual flow value is interpolated from validneighbor points above and below.

A further smoothing step can be used. For example, a median filter ofwidth three can be used followed by a averaging filter of width three.This gives the residual flow:

-   -   {right arrow over (μ)}        which is known to be:

$\begin{matrix}{\overset{->}{\mu} = {\frac{H}{Z}\frac{T_{z}}{d_{\pi}^{\prime}}\left( {\overset{->}{e} - {\overset{->}{p}}_{w}} \right)}} & (9)\end{matrix}$where H is the height of the point from the reference frame. Given theresidual flow for each point along the path the equation can be solvedfor H.

While it is convenient to perform the fine tracking in pixel and subpixel resolution it is also possible to define the search areas in termsof height above or below the plane. For example instead of a search from−2 pixels to 2 pixels with subpixel resolution at 0.1 pixel accuracy, itis possible to search for a height between −0.2 m and 0.2 m at 0.01 mincrements. A search for a height of between −0.2 m and 0.2 m at 0.01 mincrements requires translating the height to a pixel shift andperforming the normalized cross correlation. The search is moreexpensive but allows imposing metric smoothness constraints in theinitial cost function.

The search can also allow a method for combining information from threeor more motions.

Consider a sequence of three frames 1, 2 and 3.

-   -   1. Warp frames 1 and 3 towards frame 2.    -   2. Track points and keep valid points that tracked well from 1        to 2 and 3 to 2.    -   3. Perform RANSAC, picking 4 points from image 2 and computing        homographies from images 1 and 3. However the inlier count is        the minimum of inliers from the mapping 1 to 2 and 3 to 2.    -   4. Compute final homographies from 1 to 2 and 3 to 2 and warp        images.    -   5. For points along the wheel tracks, perform a search for best        height from the reference plane. For each height compute the        residual pixel motion from 2 to 1 and from 2 to 3 separately,        compute the normalized correlation scores separately and average        (or minimum or maximum). Alternatively one can compute a        combined normalized correlation score.    -   6. Pick best score.        Multi-Frame Analysis

The system can detect shape features and bumps that are a fewcentimeters high at distance of greater than 10 m. Naturally there isalso some noise in the system and spurious bumps are detected. Howeverreal shape features will move consistently with the movement of thevehicle, while spurious shape features due to noise will appear randomlyor might be stationary in the image if they are due to imagingartifacts. Shape features due to moving objects will also not moveconsistently with the vehicle.

It is therefore, useful to accumulate information over time. One methodwould be to use plane information and the road profile information tocreate a road profile in 3D (X,Y,Z) coordinates and then use the egomotion of the vehicle to transform the model from frame to frame. AKalman filter could be used.

Another method uses the homography itself to carry information fromframe to frame over time. Using the homography itself to carryinformation from frame to frame over time takes advantage of the factthe the road profile is defined on a road surface and the actualdeviations due to the shape features are within 1 or 2 pixels, muchsmaller than the size of the surface regions or patches are beingconsidered. The basic multi-frame algorithm is as follows:

1. Assume a multi-frame road profile has been computed for frame n−m,where m is often equal 1 but might be larger if computing a profile foreach frame is not required.

2. Compute the single frame road profile for frame n using frame n andframe n−k where k might not be equal to m. Typically k is chosen so thatthe vehicle motion is above a certain value such as one meter.

3. The multi-frame profile and the single plane profile use differentreference planes. The different reference planes are often very similarsince they are determined by the same road with significant areas ofoverlap but when passing over a speed bump the assumption that thereference frames are very similar breaks down. It is therefore importantto compensate for passing over a speed bump by:

(a) Let π_(m) be the reference plane of the multi-frame model and letπ_(m) be the reference plane of the single frame model.

(b) For each point along the vehicle path (x, y), compute thecorresponding (X, Y, Z) point on the plane π_(m). Then compute thedistance from the point (X, Y, Z) to the plane π_(n).

(c) The distance from the point (X, Y, Z) to the plane π_(n) is added tothe road profile for that point along the path.

4. Compute the homography matrix (H_(m)) of the road between frames nand n−m (if m=k we can reuse the results).

5. Use the inverse of H_(nm) to transform the path coordinates (x1, y1)from frame n−m to frame n, which gives the path from frame n−m in thecoordinates of frame n, (x1_(h), y1_(h)).

-   p1=[x1,y1,ones(size(y1))];-   p2=p1*inv(Hnm)′;-   p2=[p2(:,1)./p2(:,3),p2(:,2)./p2(:,3),p2(:,3)./p2(:,3)];-   x1h=p2(:,1);-   y1h=p2(:,2);

6. Interpolate the values of the multi-frame road profile (ProfL₁) andconfidence values (VS₁) to the coordinates of the path of frame n, (x2,y2):

-   ProfL_1Interp=interp1(y1h,ProfL_1,y2,‘linear’,‘extrap’)′;-   VS_1Interp=interp1(y1h,VS_1,y2,′‘linear’,‘extrap’)′;

In the above code it is assumed that only small lateral changes in thepath. If large lateral changes are expected then it is possible toreduce the multi-frame confidence at point (i) by a function of thedifference (x1h(i)-x2(i)).

7. The new multi-frame profile is a weighted average between the warpedmulti-frame profile and the current single frame profile:

-   ProfL_1=(a*VS.*ProfL+(1−a)*VS_1Interp.*ProfL_1Interp)./(a*VS+(1−a)*VS_1Interp);

Weighting is based on the confidence scores and a time factor (a). Notethat this Matlab code. ProfL_1, VS, ProfL, VS_1Interp and ProfL_1Interpare all vectors and that the weighted average is performed for eachelement along the vector ProfL_1.

8. The multi-frame confidence is also computed:VS_1=max(1,(a*VS+(1−a)*VS_1Interp));

Note that the single frame confidence value (VS) as defined is a vectorof numbers between zero and 11 (assuming a path of width 0.5 m sampledevery 0.05 m). The max function ensures that the multi-frame confidenceis non zero for every point in the vector.

Although embodiments of the present invention are presented in thecontext of driver assistance applications, embodiments of the presentinvention may be equally applicable in other real time signal processingapplications and/or digital processing applications, such ascommunications, machine vision, audio and/or speech processing asexamples.

The indefinite articles “a”, “an” is used herein, such as “an image ”has the meaning of “one or more” that is “one or more images”.

Although selected features of the present invention have been shown anddescribed, it is to be understood the present invention is not limitedto the described features. Instead, it is to be appreciated that changesmay be made to these features without departing from the principles andspirit of the invention, the scope of which is defined by the claims andthe equivalents thereof.

What is claimed is:
 1. A computerized method for detecting a verticaldeviation of a road surface, the method performed by a driver assistancesystem mountable in a host vehicle while the host vehicle is moving,wherein the driver assistance system includes a camera operativelyconnectible to a processor, the method comprising: capturing from thecamera a plurality of consecutive image frames including a first imageof the road and a second image of the road; based on the host vehiclemotion, warping the second image toward the first image to producethereby a warped second image; tracking image points of the road in thefirst image and corresponding image points of the road in the warpedsecond image; computing optical flow between the warped second image tothe first image; comparing the optical flow with an optical flow basedon a planar or bi-quadratic road surface model of the road to producethereby a residual optical flow; and computing the vertical deviation ofthe road surface from said residual optical flow.
 2. The method of claim1, further comprising, prior to the tracking: selecting a grid of aplurality of image points in the first image.
 3. The method of claim 2,further comprising: locating a plurality of image patches around theimage points of the selected grid and performing the tracking betweenthe image patches in the first image and corresponding second imagepatches in the warped second image.
 4. The method of claim 1, whereinsaid warping includes: aligning said second image toward said firstimage by adjusting for at least one image shift due to at least onemotion of the vehicle relative to the road, wherein said at least onemotion is selected from the group consisting of: yaw, pitch and roll;and adjusting for relative scale change between said second image andsaid first image, wherein said relative scale change arises fromdifferent distances to the camera.
 5. The method of claim 1, furthercomprising: fitting the tracked points to a homography.
 6. The method ofclaim 5, further comprising: using the homography to correct the warp ofthe second image toward the first image.
 7. The method of claim 1,wherein said image points are located in a fixed distribution on theimage of the road surface.
 8. The method of claim 5, further comprising:refined warping said warped second image frame toward said first imageframe thereby correcting said warping by using said homography andproducing a refinely warped second image frame.
 9. The method of claim8, further comprising: computing optical flow between said refinelywarped second image frame and said first image frame; prior to saidcomparing said optical flow with said optical flow based on the surfacemodel.
 10. A driver assistance system mountable in a host vehicle,wherein the driver assistance system includes a camera operativelyconnectible to a processor, wherein the driver assistance systemoperates while the host vehicle is moving to detect a vertical deviationof a road surface, the driver assistance system configured to: capturefrom the camera a plurality of consecutive image frames including afirst image of the road and a second image of the road; based on thehost vehicle motion, warp the second image toward the first image toproduce a warped second image; track image points of the road in thefirst image and corresponding image points of the road in the warpedsecond image; estimate optical flow between the warped second image tothe first image; compare the optical flow with an optical flow based ona planar or bi-quadratic surface model of the road to produce a residualoptical flow; and compute the vertical deviation of the road surfacefrom said residual optical flow.
 11. The driver assistance system ofclaim 10, further configured to: select a grid of a plurality of imagepoints in the first image.
 12. The driver assistance system of claim 11,further configured to: locate a plurality of image patches around theimage points of the selected grid and performing the tracking betweenthe image patches in the first image and corresponding second imagepatches in the warped second image.
 13. The driver assistance system ofclaim 10, wherein the warp aligns the second image toward the firstimage.
 14. The driver assistance system of claim 13, wherein the warpadjusts for at least one image shift due to at least one motion of thevehicle relative to the road, wherein said at least one motion isselected from the group consisting of: yaw, pitch and roll.
 15. Thedriver assistance system of claim 14 wherein the warp adjusts forrelative scale change between said second image and said first image,wherein said relative scale change arises from different distances tothe camera.
 16. The driver assistance system of claim 13, furtherconfigured to: fit the tracked points to a homography.
 17. The driverassistance system of claim 14, further configured to: use the homographyto correct the warp of the second image toward the first image.